package com.suray.rgv.son.core;

import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.utils.LoggerType;
import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.basic.wcs.utils.thread.BaseThread;
import com.suray.commin.core.utils.StringUtils;
import com.suray.commin.exception.OrderExecuteException;
import com.suray.commin.exception.Rgv60TaskException;
import com.suray.rgv.son.comm.RgvSonComm;
import com.suray.rgv.son.comm.RgvSonReadData;
import com.suray.rgv.son.comm.RgvSonWriteData;
import com.suray.rgv.son.data.RgvReadRecMessage;
import com.suray.rgv.son.data.RgvSonControlBean;
import com.suray.rgv.son.task.RgvSonTask;
import com.suray.rgv.son.util.*;
import com.suray.rgv.util.ErrorInfo;
import com.suray.rgv.util.RgvLogger;
import com.suray.rgv.util.RgvParamConfig;
import com.suray.socket.SocketInterface;
import com.suray.socket.type.SocketType;
import com.suray.type.RgvSonTaskResult;
import com.suray.type.RgvSonTaskStatus;
import com.suray.wcs.res.enums.PalletStatus;
import com.suray.wcs.res.enums.RgvStatus;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/**
 * 子车核心
 */
public class RgvSonCore extends BaseThread {

    private RgvSonInfo rgvSonInfo;

    private volatile RgvSonTask task;

    private final int START_SON_RGV = 1;

    private int rgvNo;

    private RgvSonComm rgvSonComm;

    private RgvSonWriteData writeData;

    private RgvSonReadData readData;

    public RgvSonCore(int rgvNo, String serialNum) {
        super(serialNum + "信息处理", RgvParamConfig.RGV_THREAD_TIME);
        this.rgvNo = rgvNo;
        rgvSonInfo = new RgvSonInfo(serialNum);
    }

    /**
     * 初始化socket
     * @return
     */
    public void initSocket(String ip, int port) {
        if (rgvSonComm != null) {
            rgvSonComm.close();
        }
        rgvSonComm = new RgvSonComm(SocketInterface.getInstance(ip,  port, SocketType.CLIENT));

        writeData = new RgvSonWriteData(rgvSonComm);
        readData = new RgvSonReadData(rgvSonComm);
    }

    /**
     * 初始化
     */
    public void init() {
        task = null;
    }

    @Override
    protected void process() {
        try {
            rgvSonHeart();
        } catch (IOException e) {
            RgvLogger.info(LoggerType.ERROR, rgvSonInfo.series, "通讯异常", e.getMessage(), this.getClass());
            rgvSonInfo.rgvStatus = RgvSonStatus.OFF_LINE;
        }
    }


    /**
     * 获取任务序号
     */
    private volatile Integer lasTaskNum;

    private synchronized int generalActionNo() {
        int actionNo = rgvSonInfo.actNo;
        if (actionNo >= 60000) {
            actionNo = 1;
        } else {
            actionNo ++;
        }

        return actionNo;
    }

    /**
     * 获取电量
     * @return
     */
    public Float getBatSoc() {
        return Float.valueOf(rgvSonInfo.soc);
    }

    /**
     * 获取当前地码
     * @return
     */
    public String getGroundCode() {
        return rgvSonInfo.groundCode;
    }

    /**
     * 子车当前是否有指令
     */
    public boolean checkFree() {
        return task == null ? true : false;
    }

    /**
     * 写入默认速度
     */
    private int[] defaultSpeed() {
        int[] data = new int[6];
        data[0] = changeSpeedToRpm(rgvSonInfo.runAcc);
        data[1] = changeSpeedToRpm(rgvSonInfo.runDec);
        data[2] = changeSpeedToRpm(rgvSonInfo.runSpeed);
        data[3] = changeSpeedToRpm(rgvSonInfo.upDownAcc);
        data[4] = changeSpeedToRpm(rgvSonInfo.upDownDec);
        data[5] = changeSpeedToRpm(rgvSonInfo.upDownSpeed);
        return data;
    }

    private int changeSpeedToRpm(float speed) {
        return (int) (speed / RgvSonParamUtil.RGV_SON_RPM_FACT);
    }

    /**
     * 生成小车读取状态的脚本
     */
    public void rgvSonHeart() throws IOException {
        analyzeRgvSonInfo(readData.readData(RgvSonModbusAddr.READ_START_ADDR,
                RgvSonModbusAddr.READ_DATA_NUM_ADDR));

    }

    private String generalNodeListStr(List<Node> nodeList) {
        StringBuffer sb = new StringBuffer();

        for (Node node : nodeList) {
            sb.append(node.toString() + node.getResAction() + ",");
        }

        return sb.toString();
    }

    /**
     * 执行任务后返回母车 -- 新任务模式
     * 节点动作在nodeList里面
     * @param taskId
     * @param rgvSonTaskId
     * @param nodeList
     * @param pathIndexs : 最少包含起点和终点对应nodeList的下标, 中间可以添加取货、放货等动作对应的下标
     * @param startTask
     * @return
     */
    public void sendTask(Object taskId, int rgvSonTaskId, List<Node> nodeList, int[] pathIndexs, boolean startTask)
            throws IOException {
        checkNodesIsLine(nodeList);
        RgvSonControlBean control = new RgvSonControlBean();
        RgvSonDirection direction = analyzeDirection(nodeList.get(0), nodeList.get(1));
        RgvSonLogger.info(LoggerType.TASK, rgvSonInfo.series, taskId, "子车任务id：" + rgvSonTaskId + ", 任务节点:"
                + Arrays.toString(pathIndexs) + ", 任务路径:" + generalNodeListStr(nodeList) + ", 方向:" + direction,
                this.getClass());
        control.setTaskDirection(direction.getValue());
        control.setNodeNum(nodeList.size() - 1);
        Integer originalCode = (Integer.valueOf(nodeList.get(0).getSonRgvGroundCode()) << 8) + 0x32;
//        Integer originalCode = (Integer.valueOf(rgvSonInfo.groundCode) << 8) + 0x32;
        control.setOriginalCodeH(WcsFuncUtil.getHighWord(originalCode));
        control.setOriginalCodeL(WcsFuncUtil.getLowWord(originalCode));
        List<RgvSonControlBean.NodeInfo> nodeInfos = new ArrayList<>();
        List<Integer> distances = analyzeDistances(isDownOrRight(nodeList, direction), nodeList, direction);
        for (int i = 1; i < 31; i++) {
            if (i < nodeList.size()) {
                Node node = nodeList.get(i);
                Integer code = (Integer.valueOf(node.getSonRgvGroundCode()) << 8) + 0x32;
                RgvSonControlBean.NodeInfo nodeInfo = new RgvSonControlBean.NodeInfo(WcsFuncUtil.getHighWord(code),
                        WcsFuncUtil.getLowWord(code), distances.get(i - 1));
                nodeInfos.add(nodeInfo);
            } else {
                RgvSonControlBean.NodeInfo nodeInfo = new RgvSonControlBean.NodeInfo(0,
                        0, 0);
                nodeInfos.add(nodeInfo);
            }
        }
        control.setNodeInfo(nodeInfos);
        control.setTaskId(rgvSonTaskId);
        if (startTask) {
            control.setStartTask(1);
        } else {
            control.setStartTask(0);
        }
//        control.setMode("quickMove");
        generalTaskInfos(control, nodeList, pathIndexs);
        task = new RgvSonTask(taskId, rgvSonTaskId, nodeList);
        writeData.sendData(RgvSonModbusAddr.TASK_NEW_ADDR, control, RgvParamConfig.RESEND_COUNT);
    }

    private void generalTaskInfos(RgvSonControlBean control, List<Node> nodeList, int[] pathIndexs) {
        List<RgvSonControlBean.TaskInfo> taskInfos = new ArrayList<>();
        int sonRgvLocationIndex = -1;
        String rgvSonGroundCode = rgvSonInfo.groundCode;
        for (Node node : nodeList) {
            if (node.getSonRgvGroundCode().equals(rgvSonGroundCode)) {
                sonRgvLocationIndex = nodeList.indexOf(node);
                break;
            }
        }

        if (sonRgvLocationIndex == -1) {
            throw new RuntimeException("路径未匹配到子车位置" + rgvSonGroundCode + ":" + printNodes(nodeList));
        }

        RgvSonControlBean.TaskInfo taskInfo = generalTaskInfo(sonRgvLocationIndex,
                nodeList.get(sonRgvLocationIndex));
        taskInfos.add(taskInfo);
        int index = 1;

        int i = 0;
        if (pathIndexs[0] == sonRgvLocationIndex) {
            i++;
        }
        for (; i < pathIndexs.length; i++) {
            index++;
            taskInfos.add(generalTaskInfo(pathIndexs[i], nodeList.get(pathIndexs[i])));
        }

        control.setSegment(index * 2);      //=位置数*2
        control.setTaskNodeNum(index);      //=位置数

        for (; index < 10; index++) {
            taskInfo = new RgvSonControlBean.TaskInfo(0, 0);
            taskInfos.add(taskInfo);
        }

        control.setTaskInfo(taskInfos);
    }

    private String printNodes(List<Node> nodeList) {
        StringBuffer sb = new StringBuffer();
        for (Node node : nodeList) {
            sb.append(node.toString()) ;
            sb.append(node.getSonRgvGroundCode());
            sb.append("-");
        }
        return null;
    }

    private RgvSonControlBean.TaskInfo generalTaskInfo(int index, Node node) {
        Integer action = 0;
        if (node.getResAction() != null) {
            switch (node.getResAction()) {
                case PALLET_UP:
                    action = 1;
                    break;
                case PALLET_DOWN:
                    action = 2;
                    break;
                default:
                    break;
            }
        }

        return new RgvSonControlBean.TaskInfo(index, action);
    }

    /**
     * 发送任务段序号
     * @param segment
     * @return
     */
    public void generalSendSegmentScript(int segment) throws IOException {
        RgvSonLogger.info(LoggerType.TASK, rgvSonInfo.series, task.getTaskId(), "发送段序号:" + segment,
                this.getClass());
        writeData.sendData(RgvSonModbusAddr.TASK_NEW_SEND_SEGMENT_ADDR, new int[]{segment}, 5);
    }

    /**
     * 设置子车速度
     * @return
     */
    public void setRgvSonSpeedScript() throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "设置子车速度", this.getClass());
        int[] data = defaultSpeed();
        writeData.sendData(RgvSonModbusAddr.CHANGE_SPEED_ADDR, data, 5);
    }

    private boolean isDownOrRight(List<Node> nodeList, RgvSonDirection direction) {
        if (direction == RgvSonDirection.X_LEFT || direction == RgvSonDirection.X_RIGHT) {
            return nodeList.get(1).getX() - nodeList.get(0).getX() > 0 ? true : false;
        } else if (direction == RgvSonDirection.Y_LEFT || direction == RgvSonDirection.Y_RIGHT) {
            return nodeList.get(1).getY() - nodeList.get(0).getY() > 0 ? true : false;
        } else {
            throw new Rgv60TaskException("子车行驶距离计算出错：" + direction);
        }
    }

    private List<Integer> analyzeDistances(boolean downOrRight, List<Node> nodeList, RgvSonDirection direction) {
        List<Integer> distances = new ArrayList<>();
        if (downOrRight) {
            for (int i = 1; i < nodeList.size(); i++) {
                distances.add(getNodeDistance(nodeList.get(i), direction));
            }
        } else {
            for (int i = 0; i < (nodeList.size() - 1); i++) {
                distances.add(getNodeDistance(nodeList.get(i), direction));
            }
        }

        return distances;
    }

    /**
     * 检测子车是否允许执行该任务
     */
    public boolean checkSonRgvStatusCanDoTask() {
        if (rgvSonInfo.rgvStatus != RgvSonStatus.READY
                || rgvSonInfo.actionResult != RgvSonActionResult.INIT) {
            return false;
        }

        return true;
    }

    /**
     * 检测路径节点是否在一条直线上
     * @param nodes
     */
    private void checkNodesIsLine(List<Node> nodes) {
        if (nodes.size() < 2) {
            throw new Rgv60TaskException("子车行驶任务节点数量小于2，不能执行任务" + WcsFuncUtil.listToString(nodes));
        }

        Node nodePre = nodes.get(0);
        Node nodeAft = nodes.get(1);
        if (nodePre.getX() == nodeAft.getX()) {
            for (int i = 2; i < nodes.size(); i++) {
                nodePre = nodeAft;
                nodeAft = nodes.get(i);
                if (nodePre.getX() != nodeAft.getX()) {
                    throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(nodes));
                }
            }
        } else if (nodePre.getY() == nodeAft.getY()) {
            for (int i = 2; i < nodes.size(); i++) {
                nodePre = nodeAft;
                nodeAft = nodes.get(i);
                if (nodePre.getY() != nodeAft.getY()) {
                    throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(nodes));
                }
            }
        } else {
            throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(nodes));
        }
    }

    /**
     * 获取位移量
     * @param node
     * @param direction
     * @return
     */
    private Integer getNodeDistance(Node node, RgvSonDirection direction) {
        if (direction == RgvSonDirection.X_LEFT || direction == RgvSonDirection.X_RIGHT) {
            return node.findOffX();
        } else if (direction == RgvSonDirection.Y_LEFT || direction == RgvSonDirection.Y_RIGHT) {
            return node.findOffY();
        }

        throw new Rgv60TaskException("错误的行驶方向：" + direction);
    }

    public void setRgvDirectionX(Boolean rgvDirection) {
        rgvSonInfo.rgvDirectionX = rgvDirection;
        RgvSonLogger.info(LoggerType.INFO, rgvDirection, "", "x增大为右?" + rgvDirection, this.getClass());
    }

    public void setRgvDirectionY(Boolean rgvDirection) {
        rgvSonInfo.rgvDirectionY = rgvDirection;
        RgvSonLogger.info(LoggerType.INFO, rgvDirection, "", "y增大为右?" + rgvDirection, this.getClass());
    }

    /**
     * 分析子车行驶方向
     * x增大或y增大为右，否则为左(前端配置)
     * @param nodePre
     * @param nodeAft
     * @return
     */
    private RgvSonDirection analyzeDirection(Node nodePre, Node nodeAft) {
        try {
            if (nodePre.getX() == nodeAft.getX()) {
                if (rgvSonInfo.rgvDirectionY) {
                    return nodeAft.getY() - nodePre.getY() > 0 ? RgvSonDirection.Y_RIGHT : RgvSonDirection.Y_LEFT;
                } else {
                    return nodeAft.getY() - nodePre.getY() > 0 ? RgvSonDirection.Y_LEFT : RgvSonDirection.Y_RIGHT;
                }
            } else if (nodePre.getY() == nodeAft.getY()) {
                if (rgvSonInfo.rgvDirectionX) {
                    return nodeAft.getX() - nodePre.getX() > 0 ? RgvSonDirection.X_RIGHT : RgvSonDirection.X_LEFT;
                } else {
                    return nodeAft.getX() - nodePre.getX() > 0 ? RgvSonDirection.X_LEFT : RgvSonDirection.X_RIGHT;
                }
            } else {
                throw new Rgv60TaskException("节点信息错误，不在一条直线上:" + nodePre + "," + nodeAft);
            }
        } catch (IndexOutOfBoundsException e) {
            throw new Rgv60TaskException("子车行驶路径错误，只有一个节点:" + nodePre + "," + nodeAft);
        } catch (NullPointerException e) {
            if (rgvSonInfo.rgvDirectionY == null || rgvSonInfo.rgvDirectionX == null) {
                throw new NullPointerException("请在仓库管理里面配置子车行驶方向");
            }

            throw e;
        }
    }

    private int setNormalSpeed(int[] data, int index) {
        data[index++] = changeSpeedToRpm(rgvSonInfo.runAcc);
        data[index++] = changeSpeedToRpm(rgvSonInfo.runDec);
        data[index++] = changeSpeedToRpm(rgvSonInfo.runSpeed);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownAcc);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownDec);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownSpeed);
        return index;
    }

    private int setSlowSpeed(int[] data, int index) {
        data[index++] = changeSpeedToRpm(rgvSonInfo.runSpeedMoveAcc);
        data[index++] = changeSpeedToRpm(rgvSonInfo.runSpeedMoveDec);
        data[index++] = changeSpeedToRpm(rgvSonInfo.runSpeedMove);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownAcc);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownDec);
        data[index++] = changeSpeedToRpm(rgvSonInfo.upDownSpeed);
        return index;
    }

    /**
     * 正常动作，需要写入启动
     * @param taskId
     * @param action
     * @throws IOException
     */
    private void normalAction(Object taskId, RgvSonAction action) throws IOException {
        int[] data = new int[9];

        int actionNo = generalActionNo();
        data[0] = actionNo;
        data[1] = START_SON_RGV;
        int index = setNormalSpeed(data, 2);
        data[index++] = action.getValue();
        task = new RgvSonTask(taskId, actionNo, null);
        writeData.sendData(RgvSonModbusAddr.ACTION_START_ADDR, data, 5);

        while (true) {
            checkException();

            if (rgvSonInfo.actNo.equals(actionNo)) {
                if (rgvSonInfo.actionResult == RgvSonActionResult.ACTION_OK) {
                    break;
                } else if (rgvSonInfo.actionResult == RgvSonActionResult.ACTION_FAIL) {
                    throw new OrderExecuteException(rgvNo, rgvSonInfo.getErrorCode());
                }
            }

            WcsFuncUtil.delay(RgvParamConfig.RGV_ACTION_CHECK_CYCLE_TIME);
        }

        actionResultOk();
    }

    /**
     * 正常行驶指令
     * @param taskId
     * @param action
     */
    private void moveAction(Object taskId, RgvSonAction action, RgvSonDirection direction) throws IOException {
        int[] data = new int[10];

        int actionNo = generalActionNo();
        data[0] = actionNo;
        data[1] = START_SON_RGV;
        int index = setNormalSpeed(data, 2);
        data[index++] = action.getValue();
        data[index] = direction.getValue();
        task = new RgvSonTask(taskId, actionNo, null);
        writeData.sendData(RgvSonModbusAddr.ACTION_START_ADDR, data, 5);
    }

    /**
     * 慢速行驶指令
     * @param taskId
     * @param action
     */
    private void slowMoveAction(Object taskId, RgvSonAction action, RgvSonDirection direction) throws IOException {
        int[] data = new int[10];

        int actionNo = generalActionNo();
        data[0] = actionNo;
        data[1] = START_SON_RGV;
        int index = setSlowSpeed(data, 2);
        data[index++] = action.getValue();
        data[index] = direction.getValue();
        task = new RgvSonTask(taskId, actionNo, null);
        writeData.sendData(RgvSonModbusAddr.ACTION_START_ADDR, data, 5);
        WcsFuncUtil.delay(20);
        actionResultOk();
    }

    /**
     * 校准
     * @param taskId
     */
    public void calibration(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "校准", this.getClass());
        normalAction(taskId, RgvSonAction.CALIBRATION);
    }

    /**
     * 按速度行驶
     * @return
     * @param taskId
     */
    public void moveSpeedAction(Object taskId, RgvSonDirection direction) throws IOException {
//        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "按速度行驶:" + direction, this.getClass());
        slowMoveAction(taskId, RgvSonAction.MOVE_BY_SPEED, direction);
    }

    /**
     * 按速度停止
     * @param taskId
     * @return
     */
    public void stopMoveSpeedAction(String taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "按速度停止", this.getClass());
        normalAction(taskId, RgvSonAction.STOP_MOVE_BY_SPEED);
    }

    /**
     * 托盘按速度升
     * @return
     * @param taskId
     * @param up
     */
    public void palletUpBySpeed(Object taskId, boolean up) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "按速度顶升：" + up, this.getClass());
        normalAction(taskId, RgvSonAction.PALLET_UP_BY_SPEED);
    }

    /**
     * 停止托盘按速度升
     * @return
     * @param taskId
     */
    public void stopPalletUpBySpeed(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "停止按速度顶升", this.getClass());
        normalAction(taskId, RgvSonAction.STOP_PALLET_UP_BY_SPEED);
    }

    /**
     * 充电
     * @param taskId
     */
    public void startCharge(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "充电", this.getClass());
        normalAction(taskId, RgvSonAction.CHARGE);
    }

    /**
     * 停止充电
     * @param taskId
     * @throws IOException
     */
    public void stopCharge(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "停止充电", this.getClass());
        normalAction(taskId, RgvSonAction.STOP_CHARGE);
    }

    /**
     * 按位移量行驶
     * @return
     * @param taskId
     */
    public void moveByDistance(Object taskId, RgvSonDirection direction) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "按位移行驶：" + direction, this.getClass());
        moveAction(taskId, RgvSonAction.MOVE_BY_DISTANCE, direction);
    }

    /**
     * 清除行驶位移量
     * @return
     * @param taskId
     */
    public void clearMoveOdo(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "清除行驶位移量", this.getClass());
        normalAction(taskId, RgvSonAction.CLEAR_MOVE_ODO);
    }

    /**
     * 清除升降次数
     * @return
     * @param taskId
     */
    public void clearPalletOdo(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "清除升降次数", this.getClass());
        normalAction(taskId, RgvSonAction.CLEAR_PALLET_ODO);
    }

    /**
     * 托盘校准
     * @return
     * @param taskId
     */
    public void palletCalibration(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "托盘校准", this.getClass());
        normalAction(taskId, RgvSonAction.PALLET_CALIBRATION);
    }

    /**
     * 清除任务
     * @return
     * @param taskId
     */
    public void clearTask(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "清除任务", this.getClass());
//        normalAction(taskId, RgvSonAction.CLEAR_TASK);
        this.task = null;
    }

    /**
     * 动作结果确认
     * @return
     */
    public void actionResultOk() throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "动作结果确认", this.getClass());
        this.task = null;
        writeData.sendData(RgvSonModbusAddr.ACTION_RESULT_CONFIRM_ADDR, new int[]{1}, 5);
    }

    /**
     * 清除故障
     * @return
     */
    public void clearError() throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "清除故障", this.getClass());
        writeData.sendData(RgvSonModbusAddr.ERROR_RECOVERY_ADDR, new int[]{0b11}, 5);
        this.task = null;
    }

    public void taskStart() throws IOException {
        RgvSonLogger.info(LoggerType.TASK, rgvSonInfo.series, RgvSonModbusAddr.TASK_NEW_START_ADDR, "任务启动",
                this.getClass());
        writeData.sendData(RgvSonModbusAddr.TASK_NEW_START_ADDR, new int[]{1}, 5);
    }

    /**
     * 解析子车数据
     * @param message
     */
    public void analyzeRgvSonInfo(RgvReadRecMessage message) {
        rgvSonInfo.rgvReadyStatus = RgvSonStatus.parseStatus(message.getRunStatus());
        rgvSonInfo.actNo = message.getCurCmdNo();
        rgvSonInfo.actionResult = RgvSonActionResult.parseResult(message.getActionResult());
        rgvSonInfo.curSpeed = message.getCurSpeed();
        if (rgvSonInfo.curSpeed > (0x10000 / 2)) {
            rgvSonInfo.curSpeed -= 0x10000;
        }
        int groundCode = WcsFuncUtil.combineToDWord(message.getCurCodeL(),
                message.getCurCodeH());
        if (groundCode == 0xFFFF1F) {
            rgvSonInfo.groundCode = "";
        } else {
            rgvSonInfo.groundCode = String.format("%06d", groundCode);
        }
        rgvSonInfo.errorCode = message.getActionError();
        rgvSonInfo.warnCode = message.getWarn();

        int value = message.getAddr0xD();
        rgvSonInfo.soc = value & 0xFF;
        value = value >> 8;
        rgvSonInfo.palletStatus = PalletStatus.parseStatus(value & 0x11);
        value = value >> 2;
        rgvSonInfo.isCharge = (value & 0x1) == 1 ? true : false;
        value = value >> 1;
        rgvSonInfo.isSpeedMode = (value & 0x1) == 1 ? true : false;

        rgvSonInfo.taskId = message.getTaskNo();
        rgvSonInfo.taskStatus = RgvSonTaskStatus.parseValue(message.getTaskStatus());
        rgvSonInfo.finishSegment = message.getFinishSegment();
        rgvSonInfo.taskResult = RgvSonTaskResult.parseValue(message.getTaskResult());
        rgvSonInfo.taskErrorCode = message.getTaskError();

        if (task != null && task.getTaskId() != null && task.getTaskId().equals(rgvSonInfo.taskId)) {
            if (rgvSonInfo.taskResult == RgvSonTaskResult.SUCCESS) {
                //任务成功
                RgvSonLogger.info(LoggerType.TASK, task.getTaskId(), rgvSonInfo.series + " 子车任务完成",
                        null, this.getClass());
            } else if (rgvSonInfo.taskResult == RgvSonTaskResult.FAIL) {
                //任务失败
                RgvSonLogger.info(LoggerType.TASK, task.getTaskId(), rgvSonInfo.series + " 子车任务异常",
                        "异常码:" + rgvSonInfo.taskErrorCode, this.getClass());
            }
        }

        switch (rgvSonInfo.actionResult) {
            case ACTION_FAIL:
            case ACTION_ERROR:
                rgvSonInfo.rgvStatus = RgvSonStatus.ERROR;
                break;
            case INIT:
                rgvSonInfo.rgvStatus = RgvSonStatus.READY;
                break;
            case ACTION_OK:
                rgvSonInfo.rgvStatus = RgvSonStatus.NOT_READY;
                break;
            case INVALID:
            default:
                break;
        }

        analyzeSonRgvStatus();

        checkTaskStatus();

        rgvSonInfo.time = System.currentTimeMillis();
    }

    public long getRgvSonDataTime() {
        return rgvSonInfo.time;
    }

    /**
     * 解析子车状态
     */
    private void analyzeSonRgvStatus() {
        if (rgvSonInfo.isCharge) {
            rgvSonInfo.rgvStatus = RgvSonStatus.CHARGING;
        } else if (rgvSonInfo.isSpeedMode) {
            rgvSonInfo.rgvStatus = RgvSonStatus.SPEED_RUN;
        } else {
            if (rgvSonInfo.rgvReadyStatus == RgvSonStatus.OFF_LINE) {
                rgvSonInfo.rgvStatus = RgvSonStatus.OFF_LINE;
            }

            switch (rgvSonInfo.taskStatus) {
                case UNREADY:
                    rgvSonInfo.rgvStatus = RgvSonStatus.NOT_READY;
                    break;
                case WAITING:
                    rgvSonInfo.rgvStatus = RgvSonStatus.WAITING;
                    break;
                case READY:
                case INVALID:
                default:
                    rgvSonInfo.rgvStatus = rgvSonInfo.rgvReadyStatus;
                    break;
            }
        }
    }

    public RgvSonInfo getRgvSonInfoData() {
        return rgvSonInfo;
    }

    /**
     * 检测任务信息
     */
    private void checkTaskStatus() {
        if (task == null) {
            return ;
        }

        if (task.getActionNo() == rgvSonInfo.actNo) {
            task.setActionResult(rgvSonInfo.actionResult);
        }
    }

    public boolean checkTaskEnd(Object taskId) {
        if (task != null) {
            if (task.getTaskId().equals(taskId)) {
                switch (task.getActionResult()) {
                    case ACTION_OK:
                        return true;
                    case ACTION_FAIL:
                        throw new OrderExecuteException(rgvNo, rgvSonInfo.getErrorCode());
                    case INVALID:
                    case INIT:
                        if (rgvSonInfo.rgvStatus == RgvSonStatus.READY) {
                            RgvSonLogger.error(LoggerType.INFO, null, rgvSonInfo.series,
                                    "检测到子车未执行任务:" + taskId, this.getClass());
                        }
                        return false;
                    default:
                        RgvSonLogger.error(LoggerType.INFO, null, rgvSonInfo.series,
                                "子车状态错误，无法判断任务是否完成:" + taskId, this.getClass());
                        return false;
                }
            } else {
                RgvSonLogger.error(LoggerType.INFO, null, rgvSonInfo.series,
                        "任务Id错误，无法确认任务状态：" + task, this.getClass());
                return false;
            }
        }

        RgvSonLogger.error(LoggerType.INFO, null, rgvSonInfo.series,
                "当前无任务:" + taskId, this.getClass());
        return false;
    }

    /**
     * 是否在母车里面
     * @return
     */
    public boolean isInMother() {
        return rgvSonInfo.groundCode.equals(rgvSonInfo.platformGroundCode);
    }

    public void changeSpeedConfig(Integer moveSpeed, Integer moveSpeedAcc, Integer moveSpeedDec) {
        if (moveSpeed != null && !moveSpeed.equals(0)) {
            rgvSonInfo.runSpeed = moveSpeed;
        }
        if (moveSpeedAcc != null && !moveSpeedAcc.equals(0)) {
            rgvSonInfo.runAcc = moveSpeedAcc;
        }
        if (moveSpeedDec != null && !moveSpeedDec.equals(0)) {
            rgvSonInfo.runDec = moveSpeedDec;
        }
    }

    public int getActionResult() {
        return rgvSonInfo.actionResult.getValue();
    }

    public String getSonGroundCodeInMother() {
        return rgvSonInfo.platformGroundCode;
    }

    public void setPlatformGroundCode(String platformGroundCode) {
        rgvSonInfo.platformGroundCode = platformGroundCode;
    }

    public PalletStatus getRgvPalletStatus() {
        return rgvSonInfo.palletStatus;
    }

    public RgvStatus getRgvStatus() {
        switch (rgvSonInfo.rgvStatus) {
            case INVALID:
                return RgvStatus.UNKNOWN;
            case OFF_LINE:
                return RgvStatus.OFF_LINE;
            case READY:
                if (rgvSonInfo.actionResult != RgvSonActionResult.INIT) {
                    return RgvStatus.RUNING;
                }
                return RgvStatus.READY;
            case NOT_READY:
                return RgvStatus.RUNING;
            case ERROR:
                return RgvStatus.ERROR;
            case CHARGING:
                return RgvStatus.CHARGING;
            case WAITING:
                return RgvStatus.NODE_STANDBY;
            default:
                break;
        }

        return RgvStatus.UNKNOWN;
    }

    public Integer getCurActionNo() {
        return task == null ? null : task.getActionNo();
    }

    public String getSeries() {
        return rgvSonInfo.getSeries();
    }

    /**
     * 子车状态改为error
     */
    public void rgvError(ErrorInfo errorInfo) {
        rgvSonInfo.rgvStatus = RgvSonStatus.ERROR;
    }

    /**
     * 检测异常
     */
    public void checkException() {
        if (rgvSonInfo.rgvStatus == RgvSonStatus.ERROR) {
            throw new OrderExecuteException(rgvNo, rgvSonInfo.getErrorCode());
        }
    }

    /**
     * 故障恢复
     * @return
     */
    public boolean recovery() throws IOException {
        clearError();
        clearTask(null);
        rgvSonInfo.rgvStatus = RgvSonStatus.READY;
        return true;
    }

    /**
     * 是否在线
     * @return
     */
    public boolean isOnline() {
        return rgvSonComm.isOnline() && rgvSonInfo.rgvStatus != RgvSonStatus.OFF_LINE;
    }

    /**
     * 数据是否失效
     * @return
     */
    public boolean isDataInfoOutTime() {
        return (System.currentTimeMillis() - rgvSonInfo.time) > RgvSonParamUtil.DATA_OUT_OF_TIME;
    }

    /**
     * 子车是否启用
     * @return
     */
    public boolean isRgvEnable() {
        return rgvSonInfo.rgvEnable;
    }

    /**
     * 启用子车
     */
    public void enableRgv() {
        rgvSonInfo.rgvEnable = true;
    }

    /**
     * 停用子车
     */
    public void disableRgv() {
        rgvSonInfo.rgvEnable = false;
    }

    public int getErrorCode() {
        return rgvSonInfo.errorCode;
    }

    public int getWarnCode() {
        return rgvSonInfo.warnCode;
    }

    /**
     * 子车是否静止
     * @return
     */
    public boolean isStatic() {
        if (rgvSonInfo.rgvStatus == RgvSonStatus.NOT_READY || rgvSonInfo.rgvStatus == RgvSonStatus.SPEED_RUN) {
            return false;
        }

        return true;
    }

    public int getRgvSonTaskErrorCode() {
        return rgvSonInfo.taskErrorCode;
    }

    public RgvSonTaskResult getRgvSonTaskResult() {
        return rgvSonInfo.taskResult;
    }

    public RgvSonTaskStatus getRgvSonTaskStatus() {
        return rgvSonInfo.taskStatus;
    }

    public int getRgvCurTaskId() {
        return rgvSonInfo.taskId;
    }

    public int getRgvTaskEndSegment() {
        return rgvSonInfo.finishSegment;
    }

    public boolean positionOk() {
        return StringUtils.isNotEmpty(rgvSonInfo.groundCode);
    }

    public void palletUp(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "托盘顶升", this.getClass());
        normalAction(taskId, RgvSonAction.GET_CARGO);
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "托盘顶升完成", this.getClass());
    }

    public void palletDown(Object taskId) throws IOException {
        RgvSonLogger.info(LoggerType.ACTION, rgvSonInfo.series, null, "托盘下降", this.getClass());
        normalAction(taskId, RgvSonAction.PUT_CARGO);
    }

    @Override
    public void endThread() {
        rgvSonComm.close();
        super.endThread();
    }
}
